/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvWorld/TrajSrc/spin_traj.cpp
 * @Description  : 实现一个旋转轨迹类，提供了创建旋转轨迹、修改旋转方向、设置进度、获取速度等功能。 
 *                 主要用于在AGV路径规划中处理旋转运动，确保运动的平滑性和准确性。
 *                 包含的方法包括创建轨迹、修改旋转方向、设置相对进度、获取速度和姿态投影等。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:23:29
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include <float.h>
#include "trajectory.h" 

//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "CSpinTraj".
//
//   CSpinTraj: The constructor.
//
// void CSpinTraj::CreateTraj(CPnt &pt, CAngle &angStart, CAngle &angEnd,
//                            CTurnDir TurnDir, float fSpinR, float fFrom, float fTo)

/**
 * @brief 创建轨迹
 *
 * 根据给定的起点、起始角度、结束角度、转向方向和范围创建旋转轨迹。
 *
 * @param point 轨迹的起点
 * @param start_angle 起始角度
 * @param end_angle 结束角度
 * @param turn_direction 转向方向
 * @param from 轨迹的开始范围
 * @param to 轨迹的结束范围
 */
void SpinTrajectory::CreateTrajectory(const Point& point, const Angle& start_angle, const Angle& end_angle,
                                      TurnDir turn_direction, float from, float to) 
{
    type_ = (short)TrajType::SPIN_TRAJ;
    turn_direction_ = turn_direction;
    start_heading_angle_ = heading_angle_ = start_angle;
    end_heading_angle_ = end_angle;
    steer_angle_ = Angle(0.523f); // 假设给30度 实际驱动单元舵角不同
    Angle turn_angle = end_angle - start_angle;
    bool is_anticlockwise = turn_direction == TurnDirTag::CounterClockwise;
    turn_angle = is_anticlockwise ? turn_angle : -turn_angle;
    turn_angle_ = turn_angle.radian_;
    if (from > turn_angle_) from = turn_angle_;
    if (to < 0 || to > turn_angle_) to = turn_angle_;
    from_ = from;
    to_ = to;
    range_ = std::fabs(to - from);
    current_point_ = point;
    curvature_ = is_anticlockwise ? FLT_MAX : -FLT_MAX;
}

/**
 * @brief 修改旋转方向
 *
 *  根据实际反馈位姿角度，修改理论规划的旋转方向
 *
 * @param start_angle 起始角度
 *
 * @return 修改成功返回true，否则返回false
 */
bool SpinTrajectory::ModifySpinDir(const Angle& start_angle) 
{
    if (std::fabs(turn_angle_) >= TO_RADIAN(5)) {
        DEBUG_OUT("ModifySpinDir: turn angle is > 5 degree; no need to modify spin dir; turn_angle_=" << turn_angle_);
        return false;
    }
    Angle turn_angle = end_heading_angle_ - start_angle;
    turn_direction_ = (turn_angle.Quadrant() <= 2) ? TurnDirTag::CounterClockwise : TurnDirTag::Clockwise;
    DEBUG_OUT("ModifySpinDir: turn_direction_ (0: COUNTER_CLOCKWISE; 1: CLOCKWISE)="
              << static_cast<int>(turn_direction_.turn_dir_tag_) << ", oldTurnAngle(radian)=" << turn_angle_);
    start_heading_angle_ = heading_angle_ = start_angle;
    bool is_anticlockwise = turn_direction_ == TurnDirTag::CounterClockwise;
    turn_angle = is_anticlockwise ? turn_angle : -turn_angle;
    turn_angle_ = turn_angle.radian_;
    if (from_ > turn_angle_) from_ = turn_angle_;
    to_ = turn_angle_;
    range_ = std::fabs(to_ - from_);
    curvature_ = is_anticlockwise ? FLT_MAX : -FLT_MAX;
    DEBUG_OUT("ModifySpinDir ok: is_anticlockwise="
              << static_cast<int>(is_anticlockwise) << ", newTurnAngle(radian)=" << turn_angle_
              << ", from_=" << from_ << ", to_=" << to_ << ", range_=" << range_);
    return true;
}

/**
 * @brief 设置相对进度
 *
 * 根据给定的速率和角度更新进度。
 *
 * @param rate 进度速率，表示当前进度相对于总进度的比例
 * @param phi 旋转角度，以弧度为单位
 */
void SpinTrajectory::SetRelProgress(float rate, float phi)
{
    progress_rate_ = rate;
    bool is_anticlockwise = turn_direction_ == TurnDirTag::CounterClockwise;
    Angle angle = is_anticlockwise ? Angle(phi) : Angle(-phi);
    heading_angle_ = start_heading_angle_ + angle;
}

/**
 * @brief 获取并返回旋转轨迹的速度
 *
 * 将速度结构体中的各个分量重置为0，并根据旋转方向设置角速度。
 *
 * @return 返回当前的速度结构体引用
 */
Velocity& SpinTrajectory::VelocityFun() 
{
    velocity_.vel_x_ = 0.0f;
    velocity_.vel_y_ = 0.0f;
    velocity_.linear_ = 0.0f;
    bool is_anticlockwise = turn_direction_ == TurnDirTag::CounterClockwise;
    velocity_.angular_ = is_anticlockwise ? progress_rate_ : -progress_rate_;
    return velocity_;
}

/**
 * @brief 根据转向角度计算速度
 *
 * 根据虚拟速度和转向方向计算速度。
 *
 * @param virtual_velocity 虚拟速度
 * @param steer_direction 转向方向
 *
 * @return 计算得到的速度
 */
Velocity SpinTrajectory::VelocityForSteerAngle(const float& virtual_velocity, int& steer_direction) 
{
    Velocity vel;
    vel.linear_ = 0.0f;
    vel.vel_x_ = 0.0f;
    vel.vel_y_ = 0.0f;
    bool is_anticlockwise = turn_direction_ == TurnDirTag::CounterClockwise;
    vel.angular_ = is_anticlockwise ? virtual_velocity : -virtual_velocity;
    steer_direction = static_cast<int>(SteerDirReport::SteerDirAuto);
    return vel;
}

/**
 * @brief 获取航向角到切线的角度
 * 原地自旋打舵后直行，舵角实际错误，用0舵角
 * 返回从航向角到切线方向的角度。
 *
 * @return 返回角度值
 */
Angle SpinTrajectory::HeadToTangent() 
{
    return steer_angle_;
}

// float CSpinTraj::GetProfileArea(float fAngularArea)
// {
//     float fThita = 0.0f;
//     // if (fabs(m_fSpinRadius) > 0.0001f)
//     // {
//     //     fThita = fAngularArea / fabs(m_fSpinRadius);
//     // }
//     return fThita;
// }

// virtual CVelocity SteerVelocity(float fVirtual) { (void)(fVirtual); return CVelocity(); }

/**
 * @brief Get the deviation between the trajectory and the specified posture
 *
 * @param posture 需要投影的姿态
 * @param data 投影的数据集
 * @param init_posture 是否初始化姿态，目前此参数未使用
 *
 * @return 总是返回 false
 */
bool SpinTrajectory::ProjectPosture(Posture& posture, ProjectData* data, bool init_posture) 
{
    (void)(posture);
    (void)(data);
    (void)(init_posture);
    bool ret = false;
    return ret;
}
